#include "so2.h"

#include <iostream>

SO2::SO2() {
    t_ = 0.0;
}

SO2::SO2(const double& t) {
	t_ = SO2::Normalize(t);
}

SO2::SO2(const SO2 & other) : t_(other.t_) {

}

SO2 SO2::operator*(SO2& other) const {
  SO2 result(*this);

  // cout << " result.matrix()0 ==== : " << result.matrix() << std::endl;

  result.t_ = SO2::exp(result.matrix() * other.matrix());

  // cout << " other.matrix() ==== : " << other.matrix() << std::endl;
  // cout << " result.matrix()1 ==== : " << result.matrix() << std::endl;

  return result;
}

SO2 SO2::operator*(const SO2& other) const {
  SO2 result(*this);

  // cout << " result.matrix()0 ==== : " << result.matrix() << std::endl;
  Eigen::Matrix2d other_rot = SO2(other).matrix();

  result.t_ = SO2::exp(result.matrix() * other_rot);

  // cout << " other.matrix() ==== : " << other.matrix() << std::endl;
  // cout << " result.matrix()1 ==== : " << result.matrix() << std::endl;

  return result;
}

SO2 SO2::operator-(const SO2& other) const{
    SO2 result(*this);
    
    Eigen::Matrix2d other_rot_transpose = SO2(other).matrix().transpose();
    result.t_ = SO2::exp(other_rot_transpose * result.matrix());

    return result;
}

// SO2 SO2::operator+=(const SO2& other) const{
//     SO2 result(*this);
//     result = SO2(SO2(other).t_ + result.t_);
//     return result;
// }

double SO2::log() {
	return SO2::Normalize(t_);
}

double SO2::inv() {
    return -SO2::Normalize(t_);
}

Eigen::Matrix2d SO2::inverse() {
   return this->matrix().transpose();
}

Eigen::Matrix2d SO2::Exp(const double& t) {
	Eigen::Matrix<double, 2, 2> R;
    R(0, 0) = std::cos(t);
    R(1, 0) = std::sin(t);
    R(1, 1) = R(0, 0);
    R(0, 1) = -R(1, 0);

    return R;
}

double SO2::exp(const Eigen::Matrix2d& R) {
    return std::atan2(R(1, 0), R(0, 0));
}

SO2 SO2::exp(const double& t) {
    return SO2(t);
}

Eigen::Matrix2d SO2::so2() {
    Eigen::Matrix<double, 2, 2> skew_lie = Eigen::Matrix<double, 2, 2>::Zero();
    skew_lie(0,1) = -t_;
    skew_lie(1,0) = t_;
    return skew_lie;
}

Eigen::Matrix2d SO2::so2(const double& v) {
    Eigen::Matrix<double, 2, 2> skew_lie = Eigen::Matrix<double, 2, 2>::Zero();
    skew_lie(0,1) = -v;
    skew_lie(1,0) = v;
    return skew_lie;
}

Eigen::Matrix2d SO2::matrix() {
    return Exp(t_);
}

double SO2::Normalize(const double &t) {
    return std::atan2(std::sin(t), std::cos(t));
}